#pragma once
#include <cmath>
#include <thread>
#include <atomic>
#include <mutex>
#include "can.h"
#include <iostream>
#include <vector>
#include <map>
#include <chrono>
#include <boost/asio.hpp>
using std::map;
using std::string;
using std::vector;
enum MOTOR_TYPE
{
    UNKOWN = 0,
    MOTOR_2006,
    MOTOR_3508,
    MOTOR_6020
};

class DJI_Motor
{
protected:
    int motor_id;
    std::atomic<int16_t> send_current; // 电流
    std::atomic<int16_t> rec_angle;    // 角度
    std::atomic<int16_t> rec_velocity; // 速度
    std::atomic<int16_t> rec_torque;   // 力矩

    std::atomic<float> sum_Angle; // 累计角度
    int last_Angle = -1;          // 上一次角度

    int MaxCurrent;
    float pos_err[2] = {};
    float vel_err[2] = {}, Iout = 0;
    float cur_err[2] = {}, cur_Iout = 0;
    float target_angle = 0;
    float target_velocity = 0;
    void Angle_PID(float Kp, float Kd);
    void Vel_PID(float Kp, float Ki);
    void Cascade_PID(float out_Kp, float out_Kd, float in_Kp, float in_Ki);
    std::function<void()> PID_callback = nullptr; // 不同PID的回调

public:
    DJI_Motor(int id) : motor_id(id),
                        send_current(0),
                        rec_angle(0),
                        rec_velocity(0),
                        rec_torque(0),
                        sum_Angle(0),
                        last_Angle(-1),
                        Iout(0),
                        target_angle(0) {}
    virtual void modify(const uint8_t *data) = 0;
    int16_t get_Pos(){return rec_angle.load();}
    int16_t get_Vel(){return rec_velocity.load();}
    int16_t get_Tor(){return rec_torque.load();}
    int16_t get_current() { return send_current.load(); }
    void set_current(int16_t current) { send_current = current; }
    void set_TargetAngle(int16_t Pos) { target_angle = Pos; }
    void set_TargetVel(int16_t Vel) { target_velocity = Vel; }
    void set_Angle_PID(float Kp  = 1500, float Kd = -6)
    {
        PID_callback = std::bind([Kp, Kd, this]()
                                 { Angle_PID(Kp, Kd); });
    }
    void set_Vel_PID(float Kp = 15, float Ki = 0.03)
    {
        PID_callback = std::bind([Kp, Ki, this]()
                                 { Vel_PID(Kp, Ki); });
    }
    void set_Cascade_PID(float out_Kp = 450, float out_Kd = -0.1, float in_Kp = 15, float in_Ki = 0.03)
    {
        PID_callback = std::bind([out_Kp, out_Kd, in_Kp, in_Ki, this]()
                                 { Cascade_PID(out_Kp,out_Kd, in_Kp, in_Ki); });
    }
    void run_PID(){
        if(PID_callback!=nullptr)
            PID_callback();
    }
};
class m2006 : public DJI_Motor
{
public:
    m2006(int i):DJI_Motor(i){
        MaxCurrent = 10000;
    }
    void modify(const uint8_t *data) override;
};
class m3508 : public DJI_Motor
{
public:
    m3508(int i):DJI_Motor(i){
        MaxCurrent = 16384;
    }
    void modify(const uint8_t *data) override;
private:
    std::atomic<int16_t> rec_temp;//温度
};
class m6020 : public DJI_Motor
{
public:
    m6020(int i):DJI_Motor(i){
        MaxCurrent = 16384;
    }
    void modify(const uint8_t *data);
private:
    std::atomic<int16_t> rec_temp;//温度
};

class CAN_BUS
{
protected:
    CAN *can_;
    vector<DJI_Motor *> motor_list;
    std::atomic<bool> running_;

    std::thread io_thread;
    boost::asio::io_context io;
    boost::asio::steady_timer t;

    can_frame send_frame_1, send_frame_2;
    // 定时器发送
    virtual void send_handler(const boost::system::error_code &e) = 0;
public:
    CAN_BUS(string can_device, int baudrate = 1000000);
    void insert(int id, MOTOR_TYPE type);
    DJI_Motor &operator[](int id)
    {
        return *motor_list[id - 1];
    }
};
// 大疆can路，用于2006和3508
class DJI_CAN_BUS : public CAN_BUS
{
private:
    // 定时器发送
    void send_handler(const boost::system::error_code &e) override;

public:
    DJI_CAN_BUS(string can_device, int baudrate = 1000000);
    ~DJI_CAN_BUS();
};
//6020的can路（未经实验）
class m6020_CAN_BUS : public CAN_BUS
{
private:
    // 定时器发送
    void send_handler(const boost::system::error_code &e) override;
public:
    m6020_CAN_BUS(string can_device, int baudrate = 1000000);
    ~m6020_CAN_BUS();
};

